/*************************************************************************
	> File Name: BaseAlgo.c
	> Author: 
	> Mail: 
	> Created Time: 2018/1/20 sat 15:12
	> Version     :
 ************************************************************************/

#include<stdio.h>
#include<stdlib.h>
#include<unistd.h>
#include<fcntl.h>
#include<string.h>
#include<math.h>


#include "AwinicLog.h"
#include "BaseAlgo.h"

#if HISI_PLATFORM
#include <xtensa/tie/xt_hifi3.h>
#include <xtensa/config/defs.h>

#define int32_32_mpy_int64(a,b)   AE_MUL32_LL(a,b)
#define int64_shift_right(value,bit) AE_SRAI64(value,bit)
#endif


#if QCOM_PLATFORM
#define  int32_32_mpy_int64(var1,var2) Q6_P_mpy_RR(var1,var2)
#define  int32_abs_int32(var)       Q6_R_abs_R_sat(var)
#define  float_mpy_float(var1,var2) Q6_R_sfmpy_RR(var1,var2)
#define  int64_right_shift(var,bit) Q6_P_asr_PI(var,bit)     //Arithmetic right shifts
#define  int32_right_shift(var,bit) Q6_R_asr_RI(var,bit)     //Arithmetic right shifts
#define  int32_neg(var)             Q6_R_neg_R(var)
#endif


void write_pcm(char* name,int32_t *buffer,int dataSize)
{
    FILE*fp;

	fp = fopen(name,"ab");
    if(fp == NULL)
	{
    	AWLOGE("fopen outout file failed \n");
		return;
	}

	fwrite(buffer,sizeof(int32_t),dataSize,fp);

	fclose(fp);
}

void write_pcm16bit(char* name,int16_t *buffer,int dataSize)
{
    FILE*fp;

	fp = fopen(name,"ab");
    if(fp == NULL)
	{
    	AWLOGE("fopen outout file failed \n");
		return;
	}

	fwrite(buffer,sizeof(int16_t),dataSize,fp);

	fclose(fp);
}


#define LOG2F_COE1 ((float)1.23149591368684)
#define LOG2F_COE2 ((float)-4.11852516267426)
#define LOG2F_COE3 ((float)6.02197014179219)
#define LOG2F_COE4 ((float)-3.13396450166353)
#define LOG2TO10   ((float)0.3010299956639812)      //1/log2(10)

 static inline float fastLog2fApprox(float inData)
{
    float outData = 0.0;
    float endValue = 0; //endValue
    int   exponent = 0;


    endValue = frexpf(fabsf(inData),&exponent);

    //outData = coe1*base2*base2*base2 + coe2*base2*base2 + coe3*base2 + coe4 + exponent
    outData  = LOG2F_COE1;
    outData *= endValue;
    outData += LOG2F_COE2;
    outData *= endValue;
    outData += LOG2F_COE3;
    outData *= endValue;
    outData += (LOG2F_COE4 + exponent);

    return outData;
}

static inline float log10fApprox(float inData)
{
    //log10 = log2(x)/log2(10)
#if QCOM_PLATFORM
	return float_mpy_float(fastLog2fApprox(inData),LOG2TO10);
#else
    return fastLog2fApprox(inData)*LOG2TO10;
#endif
}

static inline float pow10fApprox(float inData)
{
    //expf(log(10)*x)
#if QCOM_PLATFORM
	return expf(float_mpy_float(2.302585092994,inData));
#else
    return expf(2.302585092994*inData); 
#endif
}

/***********************************************************************************************
 * *Name        :absoluteInt32
 * *Description :Compute Absolute Value
 * *Parameter   :inData , value must 32Bit int
 * *Return Value: return absolute value
 * *Version: v1.0.0
 * ********************************************************************************************/
inline int32_t absoluteInt32(int32_t inData)
{
#if QCOM_PLATFORM
	return int32_abs_int32(inData);
#else
	return ((inData>=0)?(inData):(-inData));
#endif
}
/***********************************************************************************************
 * *Name        :int32Mulint32
 * *Description :Compute int32 Multiply int32  value And convert to int32
 * *Parameter   :inData , value must 32Bit
 * *Return Value: return Multiply result
 * *Version: v1.0.0
 * ********************************************************************************************/
inline int32_t int32Mulint32(int32_t inData1,int32_t inData2,int16_t factorBit)
{
    int64_t resultMul;

#if (HISI_PLATFORM ||QCOM_PLATFORM)
    resultMul = int32_32_mpy_int64(inData1,inData2);
#else
    resultMul = (int64_t)inData1*inData2;
#endif

    if(resultMul >= 0)
    {
        resultMul=resultMul>>factorBit;
    }else{
        resultMul = -resultMul;
        resultMul=resultMul>>factorBit;
        resultMul = -resultMul;
    }

    return (int32_t)resultMul;
}

/***********************************************************************************************
 * *Name        :int32RightShift
 * *Description :Int32 Value shift right shiftBit bit
 * *Parameter   :inData , value must 32Bit
 * *Return Value: return shift result
 * *Version: v1.0.0
 * ********************************************************************************************/
inline int32_t int32RightShift(int32_t inData,int16_t shiftBit)
{
    int32_t result;

    if(inData >=0)
    {
    	result = inData>>shiftBit;
    }else{
        result = -inData;
        result = result >> shiftBit;
        result = -result;
    }
    return result;
}
/***********************************************************************************************
 * *Name        :int64RightShift
 * *Description :Int64 Value shift right shiftBit bit
 * *Parameter   :inData , value must 64Bit
 * *Return Value: return shift result
 * *Version: v1.0.0
 * ********************************************************************************************/
inline int64_t int64RightShift(int64_t inData,int16_t shiftBit)
{
    int64_t result; //,factor = -(1<<shiftBit);;

    if(inData >=0)
    {
    	result = inData>>shiftBit;
    }else{
        result = -inData;
    	result = result>>shiftBit;
        result = -result;
    }
    return result;
}

/***********************************************************************************************
 * *Name        :filterProcess
 * *Description :dataBuf filter process
 * *Parameter   :dataBuf  ,filter configuration structure ,input data number
 * *Return Value: NULL
 * *Version: v1.0.0
 * ********************************************************************************************/
void filterProcess(int32_t *data,ftCfg_t *cfg,uint16_t wordsCnt)
{
    int64_t multData[FILTERCOECNT] = {0};
    int64_t inValue,outValue;
    ftParams_t *params = &cfg->params;
    ftData_t *storeData= &cfg->data;
    uint16_t cnt;

#if (HISI_PLATFORM||QCOM_PLATFORM)
    for(cnt=0;cnt<wordsCnt;cnt++)
    {
       inValue = data[cnt];
       //y[n]=b0*a[n] + b1*a[n-1] + b2*a[n-2] + a1*y[n-1] + a2*y[n-2];
       multData[0] = int32_32_mpy_int64(data[cnt],params->coe[0]);
       multData[1] = int32_32_mpy_int64(storeData->inData[0],params->coe[1]);
       multData[2] = int32_32_mpy_int64(storeData->inData[1],params->coe[2]);
       multData[3] = int32_32_mpy_int64(storeData->outData[0],params->coe[3]);
       multData[4] = int32_32_mpy_int64(storeData->outData[1],params->coe[4]);

       outValue = multData[0]+ multData[1] + multData[2] + multData[3] + multData[4];

       outValue = int64RightShift(outValue,cfg->paramsFactorBit);

       data[cnt] = outValue;

       storeData->inData[1] = storeData->inData[0];
       storeData->inData[0] = inValue;
       storeData->outData[1]= storeData->outData[0];
       storeData->outData[0]= outValue;
    }
#else
    for(cnt=0;cnt<wordsCnt;cnt++)
    {
       inValue = data[cnt];
       //y[n]=b0*a[n] + b1*a[n-1] + b2*a[n-2] + a1*y[n-1] + a2*y[n-2];
       multData[0] = (inValue*params->coe[0]);
       multData[1] = ((int64_t)storeData->inData[0]*params->coe[1]);
       multData[2] = ((int64_t)storeData->inData[1]*params->coe[2]);
       multData[3] = ((int64_t)storeData->outData[0]*params->coe[3]);
       multData[4] = ((int64_t)storeData->outData[1]*params->coe[4]);

       outValue = multData[0]+ multData[1] + multData[2] + multData[3] + multData[4];

       outValue = int64RightShift(outValue,cfg->paramsFactorBit);


       data[cnt] = outValue;

       storeData->inData[1] = storeData->inData[0];
       storeData->inData[0] = inValue;
       storeData->outData[1]= storeData->outData[0];
       storeData->outData[0]= outValue;
    }
#endif
}

void filterInit(ftCfg_t*cfg)
{
    cfg->paramsFactorBit = FTPARAMFACTORBIT;
}
void filterReset(ftCfg_t *cfg)
{
    int i;

    for(i=0;i< FILTERDATACNT;i++)
    {
        cfg->data.inData[i] = 0;
        cfg->data.outData[i]= 0;
    }
}

void filterSetParams(ftCfg_t* cfg,ftParams_t *params)
{
    int i;
    for(i=0;i<FILTERCOECNT;i++)
    {
        cfg->params.coe[i] = params->coe[i];
    }
}
void filterGetParams(ftCfg_t* cfg,ftParams_t *params)
{
    int i;
    for(i=0;i<FILTERCOECNT;i++)
    {
        params->coe[i] = cfg->params.coe[i];
    }
}
/***********************************************************************************************
 * *Name        :peakDetectorProcess
 * *Description :find data Peak value
 * *Parameter   :dataBuf  ,peakdetector configuration structure ,input data number
 * *Return Value: NULL
 * *Version: v1.0.0
 * ********************************************************************************************/
void peakDetectorProcess(int32_t*data,peakDetCfg_t *cfg,uint16_t wordsCnt)
{
   int i;
   
   for(i=0;i<wordsCnt;i++)
   {
       data[i]=absoluteInt32(data[i]);
       if(cfg->data.dataCount%cfg->params.holdRel==0)
       {
           cfg->data.compValue = cfg->data.sectionPeakValue;
           cfg->data.sectionPeakValue = 0;
           cfg->data.dataCount = 1;
       }else{
           if(cfg->data.sectionPeakValue<data[i])
           {
               cfg->data.sectionPeakValue = data[i];
           }
           cfg->data.dataCount++;
       }

       if(data[i] > cfg->data.compValue)
           cfg->data.compValue = data[i];
       data[i] = cfg->data.compValue;
   }
}
void peakDetectorInit(peakDetCfg_t *cfg)
{
    cfg->data.dataCount = 1;
    cfg->data.sectionPeakValue = 0;
    cfg->data.compValue = 0;
}

void peakDetectorReset(peakDetCfg_t *cfg)
{
    peakDetectorInit(cfg);
}

void peakDetSetParams(peakDetCfg_t *cfg,peakDetParams_t *params)
{
    cfg->params.holdRel = params->holdRel;
    cfg->params.holdAttr = params->holdAttr;
}
void peakDetGetParams(peakDetCfg_t *cfg,peakDetParams_t *params)
{
    params->holdRel = cfg->params.holdRel; 
    params->holdAttr = cfg->params.holdAttr;
}
/***********************************************************************************************
 * *Name        :curveProcess
 * *Description :Data Curve process
 * *Parameter   :dataBuf  ,Curve configuration structure ,input data number
 * *Return Value: NULL
 * *Version: v1.0.0
 * ********************************************************************************************/
void curveProcess(int32_t *data,curveCfg_t *cfg,uint16_t wordsCnt)
{
    int32_t inData,outData;
    int32_t width = cfg->params.width;
    int32_t overshoot ;

    uint16_t cnt;
    if(cfg->params.ratio > 50)
    {
        for(cnt=0;cnt<wordsCnt;cnt++)
        {
            inData = data[cnt];
            inData =  GetMaxValue(inData,CURVE_MIN);
            inData = cfg->data.logCoe*(log10fApprox(inData) - cfg->data.logSub);  //convert to db ,20*log10(data);

            //Subtrace threshold form inData to get the amount of overshoot
            overshoot = inData - cfg->params.threshold;

            //rect = inTransition *GainCoe*(shoot + width )^2 + (1-inTransition)*max(overshoot,0)
            if(absoluteInt32(overshoot)>=width)
            {
                outData = GetMaxValue(overshoot,0);
            }else{
                inData = overshoot + width;
                inData = int32Mulint32(inData,inData,cfg->data.paramFactorBit);
                outData= int32Mulint32(cfg->params.gain,inData,cfg->data.paramFactorBit);
            }
            data[cnt] = outData;

            data[cnt] = -data[cnt];
        }
    }
    else{
        for(cnt=0;cnt<wordsCnt;cnt++)
        {
            inData = data[cnt];
            inData =  GetMaxValue(inData,CURVE_MIN);
            inData = cfg->data.logCoe*(log10fApprox(inData) - cfg->data.logSub);  //convert to db ,20*log10(data);

            //Subtrace threshold form inData to get the amount of overshoot
            overshoot = inData - cfg->params.threshold;

            //rect = inTransition *GainCoe*(shoot + width )^2 + (1-inTransition)*max(overshoot,0)
            if(absoluteInt32(overshoot)>=width)
            {
                outData = GetMaxValue(overshoot,0);
            }else{
                inData = overshoot + width;
                inData = int32Mulint32(inData,inData,cfg->data.paramFactorBit);
                outData= int32Mulint32(cfg->params.gain,inData,cfg->data.paramFactorBit);
            }
            data[cnt] = outData;


            data[cnt] = int32Mulint32(data[cnt],cfg->params.slope,cfg->data.paramFactorBit);
        }
    }

}

void curveInit(curveCfg_t *cfg)
{
    cfg->data.paramFactorBit = (CURVE_PARAM_BIT);
    cfg->data.logCoe = (int32_t)(20*(1<<cfg->data.paramFactorBit));
    cfg->data.logSub = log10fApprox(1<<CURVE_INDATA_BIT);    //inData must 
    cfg->data.mode = COMPRESS;
}
void curveReset(curveCfg_t *cfg)
{
    curveInit(cfg);
}
void curveSetParams(curveCfg_t *cfg,curveParams_t *params,int32_t threshold)
{
    cfg->params.threshold = threshold;
    cfg->params.ratio     = params->ratio;
    cfg->params.knee      = params->knee;
    cfg->params.slope     = params->slope;
    cfg->params.width     = params->width;
    cfg->params.gain      = params->gain;
}
void curveGetParams(curveCfg_t *cfg,curveParams_t *params,int32_t *threshold)
{
    *threshold        = cfg->params.threshold ;
    params->threshold = 0 ;
    params->ratio     = cfg->params.ratio     ;
    params->knee      = cfg->params.knee      ;
    params->slope     = cfg->params.slope     ;
    params->width     = cfg->params.width     ;
    params->gain      = cfg->params.gain      ;
}
/***********************************************************************************************
 * *Name        :smoothProcess
 * *Description :Data smooth process
 * *Parameter   :dataBuf  ,smooth configuration structure ,input data number
 * *Return Value: NULL
 * *Version: v1.0.0
 * ********************************************************************************************/
void smoothProcess(int32_t *data,smoothCfg_t *cfg,uint16_t wordsCnt )
{
    int32_t inData,outData;
    uint16_t count;
    float dbToint;

    for(count=0;count<wordsCnt;count++)
    {
        inData = -data[count];
        if(inData>cfg->data.state)
        {
            cfg->data.state = int32Mulint32(cfg->params.alphaAttr,cfg->data.state,cfg->data.paramFactorBit)
                               + int32Mulint32(cfg->data.beta,inData,cfg->data.paramFactorBit);
            if(cfg->data.state > inData)
            {
                cfg->data.state = inData;
            }
        }else{
            cfg->data.state = cfg->data.state - cfg->params.alphaRel;
            if(cfg->data.state < inData)
            {
                cfg->data.state = inData;
            }
        }

        outData = - cfg->data.state;
        //data[count] = - cfg->data.state;
        if(outData !=0)
        {
            dbToint = pow10fApprox(cfg->data.intToFloat*outData);
            data[count] = (int32_t)(dbToint*cfg->data.floatToint);
        }else{
            data[count] = cfg->data.floatToint;
        }
    }
}

void smoothInit(smoothCfg_t *cfg)
{
    cfg->data.state = 0;
    cfg->data.betaCoefficient = SMOOTH_ALPHA_SCALE;   //constant
    cfg->data.paramFactorBit  = SMOOTH_PARAM_BIT;
    cfg->data.floatToint      = (SMOOTH_EXPAND);
    cfg->data.intToFloat      = (1.0/((1<<SMOOTH_INDATA_BIT)*20));      // 1.0/ (2^^inputDataBit * 20 )  <--20log(data)
    cfg->data.beta            = ((cfg->data.betaCoefficient*(1<<cfg->data.paramFactorBit)) - (cfg->data.betaCoefficient*cfg->params.alphaAttr));
    //cfg->data.betaCoefficient * ((1<<cfg->data.paramFactorBit) - cfg->params.alphaAttr);

}

void smoothReset(smoothCfg_t *cfg)
{
   smoothInit(cfg);
}
void smoothSetParams(smoothCfg_t *cfg,smoothParams_t *params)
{
    cfg->params.alphaAttr = params->alphaAttr;
    cfg->params.alphaRel  = params->alphaRel;

    smoothInit(cfg);
}
void smoothGetParams(smoothCfg_t *cfg,smoothParams_t *params)
{
    params->alphaAttr     = cfg->params.alphaAttr ;
    params->alphaRel      = cfg->params.alphaRel  ;
}

/***********************************************************************************************
 * *Name        :highFreqAGCProcess
 * *Description :Data High Frequency AGC process
 * *Parameter   :dataBuf  ,High Frequency AGC configuration structure ,input data number
 * *Return Value: NULL
 * *Version: v1.0.0
 * ********************************************************************************************/
//10^((0:-0.5:-31.5)/20)
static const int32_t  highFreqAGCGainTable[GAIN_TABLE_NUM +1]={
	1048576, 989919, 934544, 882266, 832913, 786321, 742334, 700809,
	 661606, 624597, 589657, 556672, 525532, 496135, 468381, 442180,
	 417445, 394094, 372048, 351236, 331588, 313040, 295528, 278997,
	 263390, 248656, 234746, 221615, 209218, 197514, 186466, 176035,
	 166188, 156891, 148115, 139829, 132007, 124623, 117652, 111070,
	 104857,  98991,  93454,  88226,  83291,  78632,  74233,  70080,
	  66160,  62459,  58965,  55667,  52553,  49613,  46838,  44218,
	  41744,  39409,  37204,  35123,  33158,  31304,  29552,  27899
  };

#define getSign(value) (0x80000000&(value))  //Get Sign Bit
void highFreqAGCProcess(int32_t *data,highFreqAGCCfg_t *cfg,uint16_t wordsCnt)
{
	highFreqAGCParams_t *params = &cfg->params;
	highFreqAGCData_t *status = &cfg->data;

	int32_t proviewData = status->preValue;
	uint16_t cnt =0;
	int32_t finalGain = 1;
	int16_t isSingleTone = 0;

	for(cnt=0;cnt < wordsCnt; cnt++)
	{
		proviewData = status->preValue;

		if((getSign(proviewData) != getSign(data[cnt]))||(status->halfWaveCnt > HALF_WAVE_CNT))
		{
			 if(absoluteInt32(status->halfWaveCnt - status->preHalfWaveCnt)<= MaxHalfWaveDelta)
				 isSingleTone = AwinicTrue;
			 else
				 isSingleTone = AwinicFalse;
             if((AwinicTrue == isSingleTone)&&(status->halfWavePeak>=params->attractAmpTh))
             {
            	 status->attackCnt = status->attackCnt + status->halfWaveCnt;
            	 status->holdCnt = 0;
            	 status->releaseCnt = 0;
            	 status->AdjDirection = 1;

            	 if( 0 == status->gainOrder )
            	 {
            		 status->curAttackTimeTh = params->firstAttackTimeTh;
            	 }else{
            		 status->curAttackTimeTh = params->normalAttackTimeTh;
            	 }

            	 if(status->attackCnt >= status->curAttackTimeTh) //decrease gain for one step after timer expiration
            	 {
            		 status->attackCnt = 0;
            		 if(status->gainOrder >= params->maxAGCLevels - 1)
            		 {
            			 status->gainOrder = params->maxAGCLevels -1;
            		 }else{
            			 status->gainOrder += 1;
            		 }
            	 }
             }else if((AwinicTrue == isSingleTone)&&(status->halfWavePeak >= params->releaseAmpTh))
             {
            	 status->holdCnt    = 0;
            	 status->releaseCnt =0 ;
             }else //when signal below release th
             {
            	 if(AwinicFalse == isSingleTone)
            	 {
            		 status->attackCnt = 0;
            	 }

            	 if(0 == status->AdjDirection)
            	 {
            		 status->releaseCnt = status->releaseCnt + status->halfWaveCnt;
            		 status->holdCnt   = 0;
            		 status->attackCnt = 0;
            	 }else
            	 {
            		 if(status->holdCnt >= params->holdTimeTh)
            		 {
            			 status->AdjDirection = 0;
            			 status->holdCnt = 0;
            		 }else{
            			 status->holdCnt = status->holdCnt + status->halfWaveCnt;
            		 }
            	 }
             }
             //increease gain for one step after timer expiration
             if(status->releaseCnt >= params->releaseTimeTh)
             {
                  status->releaseCnt = 0;
                  if(status->gainOrder <1)
                  {
                	  status->gainOrder = 0;
                  }else{
                	  status->gainOrder = status->gainOrder - 1;
                  }
              }
              status->halfWavePeak = absoluteInt32(proviewData);
              status->preHalfWaveCnt = status->halfWaveCnt;
              status->halfWaveCnt = 1;
		}else{
			status->halfWaveCnt = status->halfWaveCnt + 1;
			if(status->halfWavePeak< absoluteInt32(proviewData))
			{
				status->halfWavePeak = absoluteInt32(proviewData);
			}
		}//end of ((getSign(proviewData) != getSign(data[cnt]))||(status->halfWaveCnt > 12000))

		finalGain = highFreqAGCGainTable[status->gainOrder];

		data[cnt] = int32Mulint32(data[cnt],finalGain,GAIN_PARAM_BIT);
		status->preValue = data[cnt];
	}
}
void highFreqAGCInit(highFreqAGCCfg_t *cfg)
{
	cfg->data.halfWaveCnt  = 0;
	cfg->data.halfWavePeak = 0;
	cfg->data.attackCnt    = 0;
	cfg->data.releaseCnt   = 0;
	cfg->data.holdCnt      = 0;
	cfg->data.AdjDirection = 0;
	cfg->data.gainOrder    = 0;
	cfg->data.preValue     = 0;
	cfg->data.curAttackTimeTh = 0;
	cfg->data.preHalfWaveCnt  = 0;
}
void highFreqAGCReset(highFreqAGCCfg_t *cfg)
{
	highFreqAGCInit(cfg);
}
void highFreqAGCSetParams(highFreqAGCCfg_t *cfg,highFreqAGCParams_t *params,uint16_t dataBitNum)
{
	highFreqAGCParams_t *OriginParams = &cfg->params;
    uint16_t moveBit = Default_DATA_BIT - dataBitNum;

    cfg->attractAmpTh = params->attractAmpTh;
    cfg->releaseAmpTh = params->releaseAmpTh;

	OriginParams->freqTh             =  params->freqTh;
	OriginParams->attractAmpTh       = (params->attractAmpTh) >> moveBit;    //keep Bit Number is equal with data Bit Number
	OriginParams->releaseAmpTh       = (params->releaseAmpTh) >> moveBit;    //keep Bit Number is equal with data Bit Number
	OriginParams->firstAttackTimeTh  =  params->firstAttackTimeTh;
	OriginParams->normalAttackTimeTh =  params->normalAttackTimeTh;
	OriginParams->releaseTimeTh      =  params->releaseTimeTh;
	OriginParams->holdTimeTh         =  params->holdTimeTh;
	OriginParams->maxAGCLevels       =  params->maxAGCLevels;

}
void highFreqAGCGetParams(highFreqAGCCfg_t *cfg,highFreqAGCParams_t *params,uint16_t dataBitNum)
{
	highFreqAGCParams_t *OriginParams = &cfg->params;
	//uint16_t moveBit = GAIN_PARAM_BIT - dataBitNum;

	params->freqTh             =  OriginParams->freqTh;
	params->attractAmpTh       =  cfg->attractAmpTh;
	params->releaseAmpTh       =  cfg->releaseAmpTh;
	params->firstAttackTimeTh  =  OriginParams->firstAttackTimeTh;
	params->normalAttackTimeTh =  OriginParams->normalAttackTimeTh;
	params->releaseTimeTh      =  OriginParams->releaseTimeTh ;
	params->holdTimeTh         =  OriginParams->holdTimeTh;
	params->maxAGCLevels       =  OriginParams->maxAGCLevels;

}
